

#include "full_cover_path_planner/full_cover_planner.h"
#include "path_smooth/points_process.h"
#include "path_smooth/SmoothPath.h"
#include "glog_func/log.h"
#include "discretized_points_smoother/fem_pos_deviation_smoother.h"

#include <ros/ros.h>
#include <cln_msgs/MarkerPoint.h>

#include <yaml-cpp/yaml.h>

using namespace global_path_planner::common;

namespace global_path_planner {
namespace ccpp {

FullCoverPlanner::FullCoverPlanner(float& radius,
                                std::string& map_dirct,
                                float& path_width,
                                std::shared_ptr<MapProcess>& map_proc,
                                std::shared_ptr<::common::planning::PathPlan>& path_plan_ptr)
                                :__robot_Radius(radius),
                                 __map_Direct(map_dirct),
                                 __path_Width(path_width),
                                 __map_Proc_Ptr(map_proc),
                                 __path_Plan_Ptr(path_plan_ptr)
{
    // 初始化参数
    InitParam();
    __polygon2d_Ptr = std::make_shared<Polygon2D>(Polygon2D());
    __cell_Decomp_Ptr = std::make_shared<CellDecomposer>(CellDecomposer(__polygon2d_Ptr));
    __bstphns_Planner_Ptr = std::make_shared<BstphdnsPlanner>(BstphdnsPlanner(__map_Proc_Ptr,
                                                                            __cell_Decomp_Ptr,
                                                                            __polygon2d_Ptr));
    __loop_Planner_Ptr = std::make_shared<LoopPlanner>(LoopPlanner(__map_Proc_Ptr,
                                                                __cell_Decomp_Ptr,
                                                                __polygon2d_Ptr));
}

FullCoverPlanner::~FullCoverPlanner()
{

}

// 初始化参数
void FullCoverPlanner::InitParam(){
    // 两点间的距离
    __path_interval = ros::param::param<float>("/full_cover_path/ptn_interval", 0.05);
    LOG(INFO) << "__path_interval: " << __path_interval;
    // 扩展半径
    __extend_Radius = ros::param::param<float>("/full_cover_path/extend_radius", 0.05);
    LOG(INFO) << "__extend_Radius: " << __extend_Radius;
    // 是否为圆弧弓字形
    __arc_Boustrophedon = ros::param::param<bool>("/full_cover_path/arc_boustrophedon", false);
    LOG(INFO) << "__arc_Boustrophedon: " << __arc_Boustrophedon ? 1 : 0;
    // 贴边距离
    __perim_Dist = ros::param::param<float>("/full_cover_path/perim_distance", 0.7);
    LOG(INFO) << "__perim_Dist: " << __perim_Dist;
    // 回字形是否平滑
    __loop_Smooth = ros::param::param<bool>("/full_cover_path/loop_smooth", false);
    LOG(INFO) << "__loop_Smooth: " << __loop_Smooth ? 1 : 0;
    // 子任务间是否需要链接
    __subtasks_Link = ros::param::param<bool>("/full_cover_path/subtasks_link", false);
    LOG(INFO) << "__subtasks_Link: " << __subtasks_Link ? 1 : 0;
    // 是否包含圆形
    __contain_Circle = false;
}

// 更新地图
void FullCoverPlanner::UpdateParam(std::string map_name,
                                  std::string map_dir,
                                  double origin_x,
                                  double origin_y,
                                  double resolution,
                                  double area,
                                  float path_width,
                                  int16_t gen_area){
    __map_Name = map_name;
    // 地图原始坐标
    __map_OriginX = origin_x;
    __map_OriginY = origin_y;
    // 地图宽度和高度
    // __map_Width = msg_ptr->map_meta.width;
    // __map_Height = msg_ptr->map_meta.height;
    // 地图分辨率
    __map_Resolution = resolution;
    // 路径宽度
    __path_Width = path_width;
    // 地图文件夹
    __map_Direct = map_dir;
    // 是否只生成规划区域面积
    __gen_Area = gen_area;
}

// 求解路径
std::map<uint8_t, std::vector<std::vector<std::pair<float, float>>>>
FullCoverPlanner::SolvedPath(int8_t& offset, int16_t& clean_mode,
                            geometry_msgs::Pose& start, bool& region_inflate,
                            std::vector<geometry_msgs::Polygon>& all_polys,
                            std::pair<bool, std::tuple<float, float, float>>& docker_pose,
                            float& all_area, std::string& error_info){
    // 1.将起始点实际值转化为像素值
    double start_x = std::round((start.position.x - __map_OriginX) / __map_Resolution);
    double start_y = std::round((start.position.y - __map_OriginY) / __map_Resolution);
    Point_2 p_start(start_x, start_y);
    std::pair<bool, Point_2> p_docker = std::make_pair(docker_pose.first, Point_2(0., 0.));
    if(docker_pose.first){
        auto x = std::round((std::get<0>(docker_pose.second) - __map_OriginX) / __map_Resolution);
        auto y = std::round((std::get<1>(docker_pose.second) - __map_OriginY) / __map_Resolution);
        p_docker.second = Point_2(x, y);
    }
    
    // 2.计算膨胀半径
    float offset_Dist = __path_interval * offset;
    float radius = (__robot_Radius + __extend_Radius) * 2 + offset_Dist; // TODO: rj.wang 动态调整会导致任务区变小
    if (clean_mode == FullCoverPlanner::E_EDGE ||
        clean_mode == FullCoverPlanner::E_EDGE_LOOP ||
        clean_mode == FullCoverPlanner::E_EDGE_BOUSTROPHEDON) { // 需要生成贴边路径
        radius = __perim_Dist;
    }

    // 3.任务类型判断是否为环形
    bool loop = (clean_mode != FullCoverPlanner::E_BOUSTROPHEDON &&
                 clean_mode != FullCoverPlanner::E_EDGE_BOUSTROPHEDON); // 弓字覆盖为false, 其余类型为true

    LOG(INFO) << "region_inflate: " << (region_inflate ? 1 : 0)
              << ", clean_mode: " << (int)clean_mode
              << ", loop: " << (loop ? 1 : 0)
              << ", offset: " << (int)offset
              << ", radius: " << radius;

    // 4.普通区域路径求解
    return NormalAreaSolvedPath(p_start, p_docker,
                                            region_inflate,
                                            clean_mode, loop,
                                            all_polys, radius,
                                            offset_Dist,
                                            all_area,
                                            error_info);
}

// 普通区域路径求解
std::map<uint8_t, std::vector<std::vector<std::pair<float, float>>>>
FullCoverPlanner::NormalAreaSolvedPath(Point_2& p_start, std::pair<bool, Point_2>& p_docker,
                                        bool& region_inflate, int16_t& clean_mode, bool& loop,
                                        std::vector<geometry_msgs::Polygon>& all_polys,
                                        float& radius, float& offset_dist,
                                        float& all_area, std::string& error_info){
    std::string output_dir = "/home/xxty/trobot/trobot_ros/ccpp_ws/src/global_path_planner/output/";

    
    // 生成除功能区之外的所有路径
    std::map<uint8_t, std::vector<std::vector<std::pair<float, float>>>> res_all_paths;
    // 普通路径（弓字形/回字形）
    std::vector<std::vector<std::pair<float, float>>> normal_pts;
    // 贴边路径
    std::vector<std::vector<std::pair<float, float>>> perim_pts;
    // 1.普通区域地图预先处理
    int piexl_radius = static_cast<int>(std::round(radius / __path_interval));
    auto pair_img = __map_Proc_Ptr->PreProcessPlanAreas(__map_Proc_Ptr->GetMapImg(), piexl_radius,
                                                        region_inflate, all_polys); // 掩膜
    cv::imwrite(output_dir + "001_PreProcessPlanAreas.png", pair_img.second);
    auto erode_global_img = __map_Proc_Ptr->PlanAreaErode(piexl_radius, __map_Proc_Ptr->GetMapImg()).second; // 腐蚀
    cv::imwrite(output_dir + "002_erode_global_img.png", erode_global_img);
    __map_Proc_Ptr->FillNoConnectedArea(erode_global_img); // 涂黑子轮廓内部区域
    cv::imwrite(output_dir + "003_FillNoConnectedArea.png", erode_global_img);

    // erode_global_img: configuration space
    // pair_img.second: 任务区外轮廓做mask, 截取erode_global_img

    // 调试代码
    bool test_gpp = ros::param::param("test_gpp", false);
    if (test_gpp) {
        std::string map_file = ::common::log::kGlobalPathPlannerLogPath + "/" + ::common::log::CurTimeString() + "_erode_mask_img.png";
        cv::Mat tmp_map;
        cv::flip(pair_img.second, tmp_map, 0);
        cv::imwrite(map_file, tmp_map);
        map_file = ::common::log::kGlobalPathPlannerLogPath + "/" + ::common::log::CurTimeString() + "_erode_global_img.png";
        tmp_map = erode_global_img.clone();
        cv::circle(tmp_map, cv::Point(CGAL::to_double(p_start.x()), CGAL::to_double(p_start.y())), 3, cv::Scalar(100));
        if (p_docker.first) {
          cv::rectangle(tmp_map,
                        cv::Point(CGAL::to_double(p_docker.second.x()) - 2, CGAL::to_double(p_docker.second.y()) - 2),
                        cv::Point(CGAL::to_double(p_docker.second.x()) + 2, CGAL::to_double(p_docker.second.y()) + 2),
                        cv::Scalar(100));
        }
        cv::flip(tmp_map, tmp_map, 0);
        cv::imwrite(map_file, tmp_map);
    }

    // 2.提取待规划的任务区和对应的轮廓组
    auto unorder_polys = getAccessAreas(p_start, loop, p_docker,
                                        pair_img.second, erode_global_img,
                                        error_info);
    // 3.根据clean_mode生成不同路径
    Point_2 pp_start = p_start;
    while(unorder_polys.size() > 0){ // 对于每一个连通域(地图polys.first、轮廓polys.second)
        // 根据当前起点计算最近的规划轮廓的参考点
        auto p_polys = IndexOfMinDistToContours(pp_start, unorder_polys);
        auto polys = unorder_polys[p_polys.first];
        unorder_polys.erase(p_polys.first);
        pp_start = p_polys.second;
        // 缓存下当前轮廓是否能规划任务
        bool planned = false;
        if(polys.second.size() <= 0) {
            LOG(WARNING) << "contours empty!";
            continue;
        } else {
            LOG(INFO) << "out contour points' size: " << polys.second.front().size();
        }
        // __gen_Area = 0时既要生成路径和面积
        if(__gen_Area == 0){
            if(clean_mode == FullCoverPlanner::E_BOUSTROPHEDON){
                // 普通弓字形
                LOG(INFO) << "BOUSTROPHEDON";
                auto point2_pts = __bstphns_Planner_Ptr->getBoustrophedonPath(pp_start, pair_img.first,
                                                                            polys.first, __path_Width,
                                                                            __path_interval, __arc_Boustrophedon,
                                                                            error_info, polys.second);
                if (!error_info.empty()) {
                    LOG(WARNING) << error_info;
                }
                if(point2_pts.size() == 0) {
                    LOG(WARNING) << "planned paths empty"; 
                    continue;
                }
                for(auto pt2s : point2_pts){
                    auto pt2s_reals = __coord_convert.Point_2sToReals(pt2s,
                                                                    __map_Resolution,
                                                                    __map_OriginX,
                                                                    __map_OriginY);
                    normal_pts.emplace_back(pt2s_reals);
                }
                planned = true;
                pp_start = point2_pts.back().back();
            }else if(clean_mode == FullCoverPlanner::E_LOOP){
                // 回字形
                LOG(INFO) << "LOOP";
                int perim = 1;
                std::reverse(polys.second[0].begin(), polys.second[0].end());
                auto unord_pixels = __loop_Planner_Ptr->getWrapperPath(polys.first,
                                                                    perim, __path_Width,
                                                                    __path_interval,
                                                                    offset_dist,
                                                                    __contain_Circle,
                                                                    __loop_Smooth,
                                                                    polys.second);
                std::unordered_map<int, std::vector<std::vector<cv::Point>>> map_unord_pixels;
                if(unord_pixels.size() > 0) map_unord_pixels[0] = unord_pixels;
                if(map_unord_pixels.size() == 0) continue;
                // 根据起点对回字形路径进行重排序
                auto sort_unord_pixels = __loop_Planner_Ptr->sortAllLoopPaths(pp_start, map_unord_pixels);
                for(auto sort_piexls:sort_unord_pixels){
                    auto sort_reals = __coord_convert.cvPoints2Reals(sort_piexls,
                                                                    __map_Resolution,
                                                                    __map_OriginX,
                                                                    __map_OriginY);
                    SmoothPairPoints(sort_reals);
                    normal_pts.emplace_back(sort_reals);
                }
                planned = true;
                pp_start = Point_2(sort_unord_pixels.back().back().x, sort_unord_pixels.back().back().y);
            }else if(clean_mode == FullCoverPlanner::E_EDGE){
                // 贴边
                LOG(INFO) << "EDGE";
                // std::reverse(polys.second[0].begin(), polys.second[0].end());
                auto piexls_pts = __loop_Planner_Ptr->getPerimPaths(polys.first,
                                                                    pair_img.first,
                                                                    __path_Width,
                                                                    __path_interval,
                                                                    polys.second);
                if(piexls_pts.size() == 0) continue;
                // 根据起点对贴边路径进行重排序
                cv::Point cv_start(CGAL::to_double(pp_start.x()), CGAL::to_double(pp_start.y()));
                auto sort_pixels_pts = __loop_Planner_Ptr->sortLoopPaths(cv_start, piexls_pts);
                // 像素值转化为实际值
                for(auto sort_piexls:sort_pixels_pts){
                    auto sort_reals = __coord_convert.cvPoints2Reals(sort_piexls,
                                                                    __map_Resolution,
                                                                    __map_OriginX,
                                                                    __map_OriginY);
                    SmoothAndProcessRiskPairPoints(sort_reals);
                    perim_pts.emplace_back(sort_reals);
                }
                planned = true;
                pp_start = Point_2(sort_pixels_pts.back().back().x, sort_pixels_pts.back().back().y);
            }else if(clean_mode == FullCoverPlanner::E_EDGE_BOUSTROPHEDON){
                // 贴边+普通弓字形
                LOG(INFO) << "EDGE_BOUSTROPHEDON";
                // std::reverse(polys.second[0].begin(), polys.second[0].end());
                auto piexls_pts = __loop_Planner_Ptr->getPerimPaths(polys.first,
                                                                    pair_img.first,
                                                                    __path_Width,
                                                                    __path_interval,
                                                                    polys.second);
                if(piexls_pts.size() == 0) continue;
                bool tmp_loop = false;
                auto tmp_unorder_polys = __map_Proc_Ptr->getAllContours(polys.first, tmp_loop);
                LOG(INFO) << "tmp_unorder_polys.size: " << tmp_unorder_polys.size();
                while(tmp_unorder_polys.size() > 0){
                    auto p_tmp_polys = IndexOfMinDistToContours(pp_start, tmp_unorder_polys);
                    auto tmp_polys = tmp_unorder_polys[p_tmp_polys.first];
                    tmp_unorder_polys.erase(p_tmp_polys.first);
                    pp_start = p_tmp_polys.second;
                    LOG(INFO) << "to getBoustrophedonPath";
                    auto point2_pts = __bstphns_Planner_Ptr->getBoustrophedonPath(pp_start, pair_img.first,
                                                                                tmp_polys.first, __path_Width,
                                                                                __path_interval, __arc_Boustrophedon,
                                                                                error_info, tmp_polys.second);
                    if(point2_pts.size() == 0) continue;
                    for(auto pt2s:point2_pts){
                        auto pt2s_reals = __coord_convert.Point_2sToReals(pt2s,
                                                                        __map_Resolution,
                                                                        __map_OriginX,
                                                                        __map_OriginY);
                        normal_pts.emplace_back(pt2s_reals);
                    }
                    pp_start = point2_pts.back().back();
                }
                // 根据起点对贴边路径进行重排序
                cv::Point cv_start(CGAL::to_double(p_start.x()), CGAL::to_double(p_start.y()));
                auto sort_pixels_pts = __loop_Planner_Ptr->sortLoopPaths(cv_start, piexls_pts);
                // 像素值转化为实际值
                for(auto sort_piexls:sort_pixels_pts){
                    auto sort_reals = __coord_convert.cvPoints2Reals(sort_piexls,
                                                                    __map_Resolution,
                                                                    __map_OriginX,
                                                                    __map_OriginY);
                    SmoothAndProcessRiskPairPoints(sort_reals);
                    perim_pts.emplace_back(sort_reals);
                }
                planned = true;
                pp_start = Point_2(sort_pixels_pts.back().back().x, sort_pixels_pts.back().back().y);
            }else if(clean_mode == FullCoverPlanner::E_EDGE_LOOP){
                // 贴边+回字形
                LOG(INFO) << "EDGE_LOOP";
                // std::reverse(polys.second[0].begin(), polys.second[0].end());
                auto piexls_pts = __loop_Planner_Ptr->getPerimPaths(polys.first,
                                                                    pair_img.first,
                                                                    __path_Width,
                                                                    __path_interval,
                                                                    polys.second);
                if(piexls_pts.size() == 0) continue;
                auto tmp_unorder_polys = __map_Proc_Ptr->getAllContours(polys.first, loop);
                int perim = 1;
                while(tmp_unorder_polys.size() > 0){
                    auto p_tmp_polys = IndexOfMinDistToContours(pp_start, tmp_unorder_polys);
                    auto tmp_polys = tmp_unorder_polys[p_tmp_polys.first];
                    tmp_unorder_polys.erase(p_tmp_polys.first);
                    pp_start = p_tmp_polys.second;

                    auto unord_pixels = __loop_Planner_Ptr->getWrapperPath(tmp_polys.first,
                                                                        perim, __path_Width,
                                                                        __path_interval,
                                                                        offset_dist,
                                                                        __contain_Circle,
                                                                        __loop_Smooth,
                                                                        tmp_polys.second);
                    std::unordered_map<int, std::vector<std::vector<cv::Point>>> map_unord_pixels;
                    if(unord_pixels.size() > 0) map_unord_pixels[0] = unord_pixels;
                    if(map_unord_pixels.size() == 0) continue;
                    // 根据起点对回字形路径进行重排序
                    auto sort_unord_pixels = __loop_Planner_Ptr->sortAllLoopPaths(pp_start, map_unord_pixels);
                    for(auto sort_piexls:sort_unord_pixels){
                        auto sort_reals = __coord_convert.cvPoints2Reals(sort_piexls,
                                                                        __map_Resolution,
                                                                        __map_OriginX,
                                                                        __map_OriginY);
                        SmoothPairPoints(sort_reals);
                        normal_pts.emplace_back(sort_reals);
                    }
                    pp_start = Point_2(sort_unord_pixels.back().back().x, sort_unord_pixels.back().back().y);
                }
                // 根据起点对贴边路径进行重排序
                cv::Point cv_start(CGAL::to_double(p_start.x()), CGAL::to_double(p_start.y()));
                auto sort_pixels_pts = __loop_Planner_Ptr->sortLoopPaths(cv_start, piexls_pts);
                // 像素值转化为实际值
                for(auto sort_piexls:sort_pixels_pts){
                    auto sort_reals = __coord_convert.cvPoints2Reals(sort_piexls,
                                                                    __map_Resolution,
                                                                    __map_OriginX,
                                                                    __map_OriginY);
                    SmoothAndProcessRiskPairPoints(sort_reals);
                    perim_pts.emplace_back(sort_reals);
                }
                planned = true;
                pp_start = Point_2(sort_pixels_pts.back().back().x, sort_pixels_pts.back().back().y);
            }
        }
        // __gen_Area == 1 或者规划成功时只生成面积, 当前已弃用
        // if((__gen_Area == 1 || planned) && (clean_mode != FullCoverPlanner::E_EDGE)){
            // all_area += __map_Proc_Ptr->CalcPlanArea(polys.second);
        // }
    }
    // 单独处理贴边的面积
    // if(clean_mode == FullCoverPlanner::E_EDGE && perim_pts.size() > 0){
        // for(auto perim_pt : perim_pts){
            // auto polys = __map_Proc_Ptr->ExtractPlanPathContours(perim_pts, 1);
	        // for(auto poly : polys) all_area += __map_Proc_Ptr->CalcPlanArea(poly.second);
        // }
        // all_area = all_area / 1.035;
    // }

    uint gen_path_type = 0;
    std::vector<std::vector<std::pair<float, float>>> res_pts;
    // 普通子任务衔接
    LOG(INFO) << "to link subtasks";
    if(normal_pts.size() > 0){
        if(__subtasks_Link) { // 参数 __subtasks_Link
            res_pts = SubtasksLink(normal_pts);
        } else {
            res_all_paths[gen_path_type] = normal_pts;
        }
    }

    // 普通任务衔接贴边任务
    LOG(INFO) << "link subtasks finish, to link normal perim";
    if(res_pts.size() > 0 && perim_pts.size() > 0) { // 若__subtasks_Link为false, res_pts为空 ...
        uint8_t result_info = 0;
        Pose start(res_pts.back().back().first, res_pts.back().back().second);
        Pose goal(perim_pts.front().front().first, perim_pts.front().front().second);
        uint8_t method = start.EuclidDistance(goal) < 50.0
                             ? ::common::planning::GraphSearch::ASTAR
                             : ::common::planning::GraphSearch::JPS;
        PoseDq p2p_path = __path_Plan_Ptr->SearchPathWithResult(start, 0.3, goal, 0.3, 0.05, result_info, method);
        if (p2p_path.empty()) {
            LOG(ERROR) << "p2p failed, result_info is " << ::common::planning::ToStringResult(result_info);
        } else {
            using namespace global_path_planner::path_smooth;
            SmoothPoseDq(p2p_path, 0.5 * __path_Plan_Ptr->DFMap().Resolution());
            double safe_dis = sqrt(__path_Plan_Ptr->DFMap().ThreshFree()) * __path_Plan_Ptr->DFMap().Resolution();
            ProcessRiskRanges(p2p_path, __path_Plan_Ptr->DFMap(), safe_dis);
            for (const auto& path_point : p2p_path) {
                res_pts.back().emplace_back(std::make_pair(path_point.X(), path_point.Y()));
            }
        }
    }
    if (res_pts.size() > 0) { // 若__subtasks_Link为false, res_pts为空 ...
        res_all_paths[gen_path_type] = res_pts;
    }

    // 贴边任务衔接
    LOG(INFO) << "link normal perim finish, to link perims";
    res_pts.clear();
    if(perim_pts.size() > 0){
        if (__subtasks_Link) {
            res_pts = SubtasksLink(perim_pts);
        } else {
            res_all_paths[++gen_path_type] = perim_pts;
        }
        if(res_pts.size() > 0) { // 若__subtasks_Link为false, res_pts为空 ...
            res_all_paths[++gen_path_type] = res_pts;
        }
    }

    // 调试代码
    LOG(INFO) << "to save debug cover img";
    cv::Mat tmp_img = __map_Proc_Ptr->GetMapImg().clone();
    for (auto it = res_all_paths.begin(); it != res_all_paths.end(); ++ it) {
        for (auto& path : it->second) {
            for (size_t i = 0; i + 1 < path.size(); ++ i) {
                auto pre_pixel = coordtrans::Realxy2Pixelxy(path.at(i), __map_Resolution, __map_OriginX, __map_OriginY);
                auto next_pixel = coordtrans::Realxy2Pixelxy(path.at(i+1), __map_Resolution, __map_OriginX, __map_OriginY);
                cv::line(tmp_img, cv::Point(pre_pixel.first, pre_pixel.second), cv::Point(next_pixel.first, next_pixel.second), cv::Scalar(0));
            }
        }
    }
    // 起点处画圆
    cv::circle(tmp_img, cv::Point(CGAL::to_double(p_start.x()), CGAL::to_double(p_start.y())), 3, cv::Scalar(100));
    std::string map_file = ::common::log::kGlobalPathPlannerLogPath + "/" + ::common::log::CurTimeString() + "_cover_img.png";
    cv::flip(tmp_img, tmp_img, 0);
    cv::imwrite(map_file, tmp_img);
    LOG(INFO) << "save debug cover img finish";

    return res_all_paths;
}

// 获取能规划的区域
std::map<int, std::pair<cv::Mat,std::vector<std::vector<cv::Point>>>>
FullCoverPlanner::getAccessAreas(Point_2& p_start, bool& loop,
                                std::pair<bool, Point_2>& p_docker,
                                cv::Mat& img, cv::Mat& erode_img,
                                std::string& error_info)
{
    // erode_img: global configuration space
    // img: 任务区外轮廓做mask, 截取erode_img

    std::map<int, std::pair<cv::Mat, std::vector<std::vector<cv::Point>>>> access_contours;
    std::map<int, std::pair<cv::Mat, std::vector<std::vector<cv::Point>>>> res_access_contours;
    // 1.获取规划区域的轮廓
    auto all_contours = __map_Proc_Ptr->getAllContours(img, loop);
    if(all_contours.size() == 0) {
        error_info = "The planned area is too small";
        return res_access_contours;
    }
    // 2.获取整张地图腐蚀后的轮廓
    std::vector<std::vector<cv::Point>> cnts;
    std::vector<cv::Vec4i> hierarchy; // index: next, prev, first_child, parent
    cv::findContours(erode_img, cnts, hierarchy, cv::RETR_EXTERNAL, cv::CHAIN_APPROX_SIMPLE);
    if(cnts.size() == 0 || hierarchy.size() == 0){
        error_info = "Failed to detect contours";
        return res_access_contours;
    }
    // 3.判断起点或者默认充电桩与规划区域是否连通
    cv::Point2f default_pose;
    if(p_docker.first){ // 有充电桩
        LOG(INFO) << "docker case";
        default_pose = cv::Point2f(CGAL::to_double(p_docker.second.x()),
                                    CGAL::to_double(p_docker.second.y()));
        // 3.1规划区域与默认充电桩是否连通
        res_access_contours = hasAccessContours(default_pose, true, erode_img, cnts, all_contours);
        if(res_access_contours.size() == 0){
            default_pose = cv::Point2f(CGAL::to_double(p_start.x()),
                                        CGAL::to_double(p_start.y()));
            res_access_contours = hasAccessContours(default_pose, false, erode_img, cnts, all_contours);
        }
    }else{ // 无充电桩
        // 3.2起点与规划区域是否连通
        LOG(INFO) << "start case";
        default_pose = cv::Point2f(CGAL::to_double(p_start.x()),
                                    CGAL::to_double(p_start.y()));
        res_access_contours = hasAccessContours(default_pose, false, erode_img, cnts, all_contours);
    }
    // 4.返回可达区域
    if(res_access_contours.size() == 0){
        error_info = "The planned area is not in the same location as the default pose";
        return res_access_contours;
    }
    return res_access_contours;
}

// 依据起点或充电桩位置是否有连通区域
std::map<int, std::pair<cv::Mat,std::vector<std::vector<cv::Point>>>>
FullCoverPlanner::hasAccessContours(cv::Point2f& default_pose, bool flag, cv::Mat& img,
                                    std::vector<std::vector<cv::Point>>& cnts,
                                    std::map<int, std::pair<cv::Mat,std::vector<std::vector<cv::Point>>>>& all_contours){
    // flag为true, default_pose为docker; 否则为start
    // cnts: 整图轮廓
    // all_contours: 任务区mask图轮廓
    std::map<int, std::pair<cv::Mat,std::vector<std::vector<cv::Point>>>> access_contours;
    int index = 0;
    int inflate_sz = 6;
    cv::Point2f tmp_default_pose = default_pose;
    
    LOG(INFO) << "to check start pose";
    auto piexl_default_pose = std::make_pair((int)tmp_default_pose.x, (int)tmp_default_pose.y);
    if(!points::CheckPixelPoint(piexl_default_pose, img)){ // 被占用
        auto defalut_pair = points::GenNearestNode(piexl_default_pose, inflate_sz, img); // 在周围邻居中找未被占用的栅格
        if(!defalut_pair.first){
            LOG(ERROR) << "start pose invalid!";
            return access_contours;
        }else{
            tmp_default_pose = cv::Point2f(defalut_pair.second.first, defalut_pair.second.second);
        }
    }
    LOG(INFO) << "check start pose finish";

    // 2.判断每个区域与充默认点是否在一个区域
    for(auto& contours : all_contours){
        LOG(INFO) << "to check contour, contour points' size: " << contours.second.second.front().size();
        cv::Point2f cur_piexlXY = cv::Point2f(contours.second.second.front().front().x,
                                            contours.second.second.front().front().y);
        // 判断轮廓顶点是否在黑斑中
        auto piexl_cur = std::make_pair((int)cur_piexlXY.x, (int)cur_piexlXY.y);
        if(!points::CheckPixelPoint(piexl_cur, img)){
            auto cur_pose_pair = points::GenNearestNode(piexl_cur, inflate_sz, img);
            if(!cur_pose_pair.first){
                LOG(WARNING) << "contour first point invalid!";
                continue;
            }else{
                cur_piexlXY = cv::Point2f(cur_pose_pair.second.first, cur_pose_pair.second.second); // 更新轮廓顶点
            }
        }
        // 通过cv::pointPolygonTest判断两点是否在同一个区域内
        for(auto i = 0; i < cnts.size(); i++){ // 完整区域的轮廓
            auto cur_result = cv::pointPolygonTest(cnts[i], cur_piexlXY, true);
            auto default_pose_result = cv::pointPolygonTest(cnts[i], tmp_default_pose, true);
            if(cur_result >= 0 && default_pose_result >= 0){
                access_contours[index++] = contours.second;
                LOG(INFO) << "got a valid contour!";
                break;
            }
        }
        LOG(INFO) << "to check contour finish";
    }
    return access_contours;
}

// 子任务间衔接
std::vector<std::vector<std::pair<float, float>>>
FullCoverPlanner::SubtasksLink(std::vector<std::vector<std::pair<float, float>>>& origin_paths){
    std::vector<std::vector<std::pair<float, float>>> res_pts;
    if(origin_paths.size() == 1) {
        res_pts = origin_paths;
    }
    else{
        std::vector<std::pair<float, float>> nor_tmp_pts = origin_paths[0];
        for(auto i = 1; i < origin_paths.size(); i++){
            uint8_t result_info = 0;
            Pose start(origin_paths[i-1].back().first, origin_paths[i-1].back().second);
            Pose goal(origin_paths[i].front().first, origin_paths[i].front().second);
            uint8_t method = start.EuclidDistance(goal) < 50.0
                                ? ::common::planning::GraphSearch::ASTAR
                                : ::common::planning::GraphSearch::JPS;
            PoseDq p2p_path = __path_Plan_Ptr->SearchPathWithResult(start, 0.3, goal, 0.3, 0.05, result_info, method);
            if(p2p_path.empty()) {
                LOG(ERROR) << "p2p failed, result_info is " << ::common::planning::ToStringResult(result_info);
            } else {
                using namespace global_path_planner::path_smooth;
                SmoothPoseDq(p2p_path, 0.5 * __path_Plan_Ptr->DFMap().Resolution());
                double safe_dis = sqrt(__path_Plan_Ptr->DFMap().ThreshFree()) * __path_Plan_Ptr->DFMap().Resolution();
                ProcessRiskRanges(p2p_path, __path_Plan_Ptr->DFMap(), safe_dis);
                for (const auto& path_point : p2p_path) {
                    nor_tmp_pts.emplace_back(std::make_pair(path_point.X(), path_point.Y()));
                }
            }
            nor_tmp_pts.insert(nor_tmp_pts.end(), origin_paths[i].begin(), origin_paths[i].end());
        }
        if(nor_tmp_pts.size() > 0) res_pts.emplace_back(nor_tmp_pts);
    }
    return res_pts;
}

// 距离当前点最近的轮廓索引
std::pair<int, Point_2> 
FullCoverPlanner::IndexOfMinDistToContours(Point_2& p_start, std::map<int, std::pair<cv::Mat,std::vector<std::vector<cv::Point>>>>& contours)
{
    int cell_idx = -1;
    double min_dist = DBL_MAX;
    Point_2 min_p;

    for(auto tmp_polys : contours){
        for(auto i = 0; i < tmp_polys.second.second[0].size(); i++){
            auto dx = CGAL::to_double(p_start.x()) - tmp_polys.second.second[0][i].x;
            auto dy = CGAL::to_double(p_start.y()) - tmp_polys.second.second[0][i].y;
            if(min_dist > std::sqrt(dx * dx + dy * dy)){
                min_dist = std::sqrt(dx * dx + dy * dy);
                cell_idx = tmp_polys.first;
                min_p = Point_2(tmp_polys.second.second[0][i].x, tmp_polys.second.second[0][i].y);
            }
        }
    }

    return std::make_pair(cell_idx, min_p);
}

/**
 * @brief 对离散点路径pair_points进行平滑
 */
bool FullCoverPlanner::SmoothPairPoints(std::vector<std::pair<float, float>>& pair_points) const {
    using namespace global_path_planner::path_smooth;
    PoseDq pose_dq{};
    // 格式转换
    for (const auto& pair_point : pair_points) {
        pose_dq.emplace_back(pair_point.first, pair_point.second);
    }

    // 平滑
    if (!SmoothPoseDq(pose_dq, 0.5 * __path_Plan_Ptr->DFMap().Resolution())) {
        return false;
    }

    // 格式转换
    for (size_t i = 0; i < pose_dq.size(); ++ i) {
        pair_points.at(i).first = pose_dq.at(i).X();
        pair_points.at(i).second = pose_dq.at(i).Y();
    }
    return true;
}

/**
 * @brief 对离散点路径pair_points进行平滑并处理碰撞风险路点
 */
bool FullCoverPlanner::SmoothAndProcessRiskPairPoints(std::vector<std::pair<float, float>>& pair_points) const {
    using namespace global_path_planner::path_smooth;
    PoseDq pose_dq{};
    // 格式转换
    for (const auto& pair_point : pair_points) {
        pose_dq.emplace_back(pair_point.first, pair_point.second);
    }

    // 平滑
    if (!SmoothPoseDq(pose_dq, 0.5 * __path_Plan_Ptr->DFMap().Resolution())) {
        return false;
    }
    // 处理风险路点
    double safe_dis = sqrt(__path_Plan_Ptr->DFMap().ThreshFree()) * __path_Plan_Ptr->DFMap().Resolution();
    ProcessRiskRanges(pose_dq, __path_Plan_Ptr->DFMap(), safe_dis);

    // 格式转换
    for (size_t i = 0; i < pose_dq.size(); ++ i) {
        pair_points.at(i).first = pose_dq.at(i).X();
        pair_points.at(i).second = pose_dq.at(i).Y();
    }
    return true;
}

}  // namespace ccpp
}  // namespace global_path_planner